package mx.com.crowdgine.util.math;

/**
 * @author Oriam De Gyves
 */
public class Quaternion {
	float x;
	float y;
	float z;
	float w;
	
	/** Creates a new quaternion initialized with zeroes.*/
	public Quaternion(){
		this.x = 0.0f;
		this.y = 0.0f;
		this.z = 0.0f;
		this.w = 0.0f;
	}
	
	/**
     * Creates a new quaternion initialized with the parameters values.
     * @param x The value to set the x component.
     * @param y The value to set the y component.
     * @param z The value to set the z component.
     * @param w The value to set the w component.
     */
	public Quaternion(float x, float y, float z, float w){
		this.x = x;
		this.y = y;
		this.z = z;
		this.w = w;
	}
	
	/**
	 * Creates a new quaternion according to it's axis/angle representation.
	 * @param axis A vector3f representing the axis component.
	 * @param angle A float value representing the angle component.
	 */
	public Quaternion(Vector3f axis, float angle){
		float s = (float) Math.sin(angle / 2);
	    w = (float) Math.cos(angle / 2);
	    x = axis.getX() * s;
	    y = axis.getY() * s;
	    z = axis.getZ() * s;
	}
	
	/**
	 * Creates a new quaternion initialized with an array
	 * @param array A size-4 array to be mapped with the x, y, z and w values.
	 * @throws RuntimeException If the length of the array is different from 4.
	 */
	public Quaternion(float[] array){
		if(array.length != 4)
            throw new RuntimeException("Quaternion constructor: Only 4 element arrays are accepted");

        x = array[0];
        y = array[1];
        z = array[2];
        w = array[3];
	}
	
	/**
	 * Creates a new quaternion initialized with a Vector4f.
	 * @param vector Vector to be mapped with the x, y, z and w values.
	 */
	public Quaternion(Vector4f vector){
		this.x = vector.getX();
		this.y = vector.getY();
		this.z = vector.getZ();
		this.w = vector.getW();
	}
	
	/**
	 * Interpolates the current quaternion with another quaternion.
	 * @param other The quaternion to interpolate with.
	 * @param t A value between 0 and 1. Represents to interpolate the quaternion with the other one.
	 */
	public void interpolate(Quaternion other, float t) {
		if (!equals(other)) {
			float d = dot(other);
			float qx, qy, qz, qw;

			if (d < 0f) {
				qx = -other.getX();
				qy = -other.getY();
				qz = -other.getZ();
				qw = -other.getW();
				d = -d;
			}else {
				qx = other.getX();
				qy = other.getY();
				qz = other.getZ();
				qw = other.getW();
			}

			float f0, f1;

			if ((1 - d) > 0.1f) {
				float angle = (float) Math.acos(d);
				float s = (float) Math.sin(angle);
				float tAngle = t * angle;
				f0 = (float) Math.sin(angle - tAngle) / s;
				f1 = (float) Math.sin(tAngle) / s;
			}else {
				f0 = 1 - t;
				f1 = t;
			}

			x = f0 * x + f1 * qx;
			y = f0 * y + f1 * qy;
			z = f0 * z + f1 * qz;
			w = f0 * w + f1 * qw;
		}

	}
	
	/**
	 * Adds a quaternion to the current quaternion. Doesn-t change the quaternion-s values.
	 * @param other The quaternion that will be added.
	 * @return A new quaternion with the result of the addition.
	 * @see #plus(Quaternion)
	 */
	public Quaternion add(Quaternion other){
        return new Quaternion(x + other.getX(),
                            y + other.getY(),
                            z + other.getZ(),
                            w + other.getW());
    }
	
	/**
	 * Adds the values of a vector4f to the current quaternion as if it were a quaternion. Doesn-t change the quaternion-s values.
	 * @param other The vector4f that will be added.
	 * @return A new quaternion with the result of the addition.
	 * @see #plus(Vector4f)
	 */
	public Quaternion add(Vector4f other){
        return new Quaternion(x + other.getX(),
                            y + other.getY(),
                            z + other.getZ(),
                            w + other.getW());
    }

	/**
     * Adds a quaternion to the current quaternion. Changes the quaternion's values.
     * @param other The quaternion that will be added.
     * @see #add(Quaternion)
     */
    public void plus(Quaternion other){
        this.x += other.getX();
        this.y += other.getY();
        this.z += other.getZ();
        this.w += other.getW();
    }
    
    /**
     * Adds a vector4f to the current quaternion as if it were a quaternion. Changes the quaternion's values.
     * @param other The vector that will be added.
     * @see #add(Vector4f)
     */
    public void plus(Vector4f other){
        this.x += other.getX();
        this.y += other.getY();
        this.z += other.getZ();
        this.w += other.getW();
    }

    /**
	 * Subtracts a quaternion to the current quaternion. Doesn-t change the quaternion-s values.
	 * @param other The vector4f that will be added.
	 * @return A new quaternion with the result of the subtraction.
	 * @see #minus(Quaternion)
	 */
    public Quaternion sub(Quaternion other){
        return new Quaternion(x - other.getX(),
                            y - other.getY(),
                            z - other.getZ(),
                            w - other.getW());
    }
    
    /**
	 * Subtracts the values of a vector4f to the current quaternion as if it were a quaternion. Doesn-t change the quaternion-s values.
	 * @param other The vector4f that will be added.
	 * @return A new quaternion with the result of the subtraction.
	 * @see #minus(Vector4f)
	 */
    public Quaternion sub(Vector4f other){
        return new Quaternion(x - other.getX(),
                            y - other.getY(),
                            z - other.getZ(),
                            w - other.getW());
    }

    /**
     * Subtracts a quaternion to the current quaternion. Changes the quaternion's values.
     * @param other The quaternionr that will be subtracted.
     * @see #sub(Quaternion)
     */
    public void minus(Quaternion other){
        this.x -= other.getX();
        this.y -= other.getY();
        this.z -= other.getZ();
        this.w -= other.getW();
    }
    
    /**
     * Subtracts a vector4f to the current quaternion as if it were a quaternion. Changes the quaternion's values.
     * @param other The vector that will be subtracted.
     */
    public void minus(Vector4f other){
        this.x -= other.getX();
        this.y -= other.getY();
        this.z -= other.getZ();
        this.w -= other.getW();
    }
    
    /**
     * Conjugates the quaternion. Doesn't change the value of the quaternion.
     * @return A quaternion with the conjugation of the original quaternion.
     * @see #conjugateThis()
     */
    public Quaternion conjugate(){
    	return new Quaternion(x, -y, -z, -w);
    }
    
    /**
     * Conjugates the quaternion. Changes the value of the quaternion.
     * @see #conjugate()
     */
    public void conjugateThis(){
    	y *= -1;
    	z *= -1;
    	w *= -1;
    }
    
    /**
     * Multiplies the quaterion with another quaternion. Doesn't change the quaternion's values.
     * @param other The quaternion to multiply with.
     * @return A new quaternion with the result of the multiplication.
     * @see #multiply(Quaternion)
     */
    public Quaternion mul(Quaternion other){
        float nx = w * other.getX() + x * other.getW() + y * other.getZ() - z * other.getY();
        float ny = w * other.getY() + y * other.getW() + z * other.getX() - x * other.getZ();
        float nz = w * other.getZ() + z * other.getW() + x * other.getY() - y * other.getX();
        float nw = w * other.getW() - x * other.getX() - y * other.getY() - z * other.getZ();
        return new Quaternion(nx, ny, nz, nw);
    }
    
    /**
     * Multiplies the quaterion with another quaternion. Changes the quaternion's values.
     * @param other The quaternion to multiply with.
     * @see #mul(Quaternion)
     */
    public void multiply(Quaternion other){
    	float nx = w * other.getX() + x * other.getW() + y * other.getZ() - z * other.getY();
        float ny = w * other.getY() + y * other.getW() + z * other.getX() - x * other.getZ();
        float nz = w * other.getZ() + z * other.getW() + x * other.getY() - y * other.getX();
        float nw = w * other.getW() - x * other.getX() - y * other.getY() - z * other.getZ();
         x = nx;
         y = ny;
         z = nz;
         w = nw;
    }
    
    /**
     * Multiplies the quaternion with a scalar value. Doesn't change the quaternion's values.
     * @param scalar The scalar value to multiply.
     * @return A quaterion with the result of the multiplication.
     * @see #scale(float)
     */
    public Quaternion mul(float scalar){
    	return new Quaternion(	x * scalar,
    							y * scalar,
    							z * scalar,
    							w * scalar);
    }
    
    /**
     * Multiplies the quaternion with a scalar value. Changes the quaternion's values.
     * @param scalar The scalar value to multiply.
     * @see #mul(float)
     */
    public void scale(float scalar){
    	x *= scalar;
    	y *= scalar;
    	z *= scalar;
    	w *= scalar;
    }
    
    /**
     *
     * @param other
     * @return
     */
    public float dot(Quaternion other){
    	return 	x * other.getX() +
				y * other.getY() +
				z * other.getZ() +
				w * other.getW();
	}

    /**
     * Calculates the length of the quaternion.
     * @return A float value with the length of the quaternion.
     */
    private float length(){
    	float len = lengthSqr();
    	if(len == 0.0f)
    		return 0.0f;
    	else
    		return (float)Math.sqrt(len);
    }
   
    /**
     * Calculates the dot operation of the quaternion with itself.
     * @return A float value with the result of the dot operation.
     */
    private float lengthSqr(){
    	return this.dot(this);
    }

    /**
     * Normalizes the quaternion. Doesn't change the vector's values.
     * @return A new quaternion with the normalized values.
     * @see #normalize()
     */
    public Quaternion norm(){
    	float len = this.length();
    	if(len != 0)
    		return this.mul(1.0f/length());
    	else
    		return new Quaternion();
    }
   
    /**
     * Normalizes the quaternion. Changes the quaternion's values.
     * @see #norm()
     */
    public void normalize(){
    	float len = this.length();
    	if(len != 0){
    		len = 1.0f/len;
    		x *= len;
    		y *= len;
    		z *= len;
    		w *= len;
    	}
    }
    
    /**
    *
    * @return The x component of the quaternion.
    */
    public float getX(){
    	return x;
    }
    
    /**
    *
    * @return The y component of the quaternion.
    */
    public float getY(){
    	return y;
    }
    
    /**
    *
    * @return The z component of the quaternion.
    */
    public float getZ(){
    	return z;
    }
    
    /**
    *
    * @return The w component of the quaternion.
    */
    public float getW(){
    	return w;
    }
    
    /**
     * Creates a string representation of the quaternion.
     * @return A string with the format: "(x,y,z,w)".
     */
    @Override
    public String toString(){
    	return "(" + x + ", " + y + ", " + z + ", " + w + ")";
    }
    
    /**
     * Checks if the quaternion is equal to another one.<br /><br />
     * A quaternion is considered equal to another if the 
     * components in one have the same values as the components in the other one.
     * @param obj The quaternion to check equality with.
     * @return True if the quaternions are equal, false otherwise.
     */
    @Override
    public boolean equals(Object obj){
    	if( obj instanceof Quaternion ){
    		Quaternion other = (Quaternion)obj;
    		
			return x == other.getX() &&
			       y == other.getY() &&
			       z == other.getZ() &&
			       w == other.getW();
		}else{
			return false;
		}
    }
}
